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Abstract 

We derive an exact deterministic nonlinear observer to compute the continuous state 
of an inertial navigation system based on partial discrete measurements, the so-called 
strapdown problem. Nonlinear contraction is used as the main analysis tool, and the 
hierarchical structure of the system physics is sytematically exploited. The paper also 
discusses the use of nonlinear measurements, such as distances to time-varying reference 
points. 



1 Introduction 



This paper derives an exact deterministic nonlinear observer to compute the continuous state 
of an inertial navigation system based on partial discrete measurements. The main analysis 
tool is nonlinear contraction theory J9j [lOl [12J [TT] [14|. Recent work on nonlinear observer 
design for mechanical systems based on nonlinear contraction theory can be found in HI |3l El 

m. 

Specifically, we consider the classical strap-down problem in inertial navigation PI [T71. 
where angular position (Euler angles) x = (ip, 9, 0) T and inertial position r are computed 
from the body turn rate uj and inertial acceleration 7, measured continuously in intrinsic (body- 
fixed) coordinates, 

{ x= H 1 LU 
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As made precise in [ 121 such a system lies at the boundary between convergence and diver- 
gence, much like a triple integrator. 

In this paper, the continuous measurements of u and 7 are augmented by discrete measure- 
ments of x and r, leading to a globally exponentially convergent nonlinear observer design. 
Such combinations of measurements are typical in inertial navigation, whether for vehicles or 
robots (see e.g. |[l6j for a recent discussion). The human vestibular system also features a 
similar structure, with otolithic organs measuring linear acceleration and semi-circular canals 
estimating angular velocity through heavily damped angular acceleration signals, an informa- 
tion then combined with visual data at much slower update rate. 

Section |3 introduces the basic observer designs. We build simple observers to compute 
(x, v, r) based on partial discrete measurements Xj and r, . In Section|3]we discuss extensions, 
such as the use of nonlinear measurements, and the effects of system disturbance and mea- 
surement disturbance ifLH . We also study the case where the inertial navigation system is ex- 
pressed in quaternion form @l|5l|6|. Section |4]presents simulation results on a 3-dimensional 
system. Brief concluding remarks are offered in Section|5j A very brief review of basic results 
in contraction theory is presented in the Appendix. 



2 Basic Algorithm 

In this section, we construct a discrete observer for system ©, which consists of a hierarchy 
of three sub-systems, mirroring the hierarchical nature of systems physics ([T]). The observer 
is based on the partial-measurements of the state x and r at a series of instants {ti}. 
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First, based on the discrete measurement Xj, compute x with the observer 

i = H~ 1 (x) oj 

(2) 

x+ = ku x7 + (1 - ku) x, 

where the first equation describes a continuous update between measurements, and the second 
equation a discrete measurement incorporation. 

Virtual displacements, which are systematically used in mathematical physics and opti- 
mization theory, also represent basic tools in contraction theory (see Appendix). Computing 
virtual displacements in © leads to 



5x = 9(H_M 5jt 
ox 



Sxf = hi <5xj 



(3) 



s&t = e, <5x+ 

6% =@i 5^ 



Based on [ 12|, define 5z = <5x with 0(x, t) = AH . This implies that 
From ©, we have 

6z = (0 + ^l^l)©- 1 Sz = 
5zf = ku 8zJ 

From hybrid contraction condition (l22b in the Appendix, if 

\ u e At * = \ u < 1 uniformly (4) 
where A H = fc^, then both 5z and <5x tend to zero exponentially. So x tends to x exponentially. 

Second, based on the discrete measurement of r, compute v with the observer 

v = A(x) 7 

vj-i = vr+i - ^: vrft + ^ (r m - r<) 
From © and the first step, we get 

|(*v) = ^x^0 
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(5) 



(6) 



Since Sir tends exponentially to a constant, we have 

"ti+l 



Using ©, this implies that 5vf +1 — > , which by continuity implies that the constant which 
Sir tends to must be zero. We thus have, exponentially, 

Sv ^ 

5vt +1 -> 

Since by design v = v is a particular solution of ©, this implies that v tends to v exponen- 
tially. 

Third, based on the discrete measurement r^, use the observer 



r = v 



(7) 



?+ = F 3i rr + (I-F 3i ) r< 
Since we know <5v tends to zero exponentially, we have 



d (St) = sir -f o 



Srf = X 3i Si i 
If \ 3i < 1, i.e. 

X 3i e Au < 1 uniformly (8) 
where \ 3i is the largest eigenvalue of F^F 3i . So r tends to r exponentially. 

Extension 1: When we compute v and r, we only use the discrete-time measurement with- 
out Xj. This allows Xj and to be measured at different instants, with the same computation. 

Extension 2: The metric can also be written & T ® = (AH) T (AH) = H T H since A is 
orthogonal. So we can simply use © = H. 

Extension 3: Assume that in © we replace the discrete update law by the more general 

x+ = F H xr + (I - F M ) Xi 
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where ©j and F 1?; commute. Then 



[ si = (e + e ^g^)©- 1 5z = o 

1 Sij = F H Sir 
The hybrid contraction condition © becomes 

Aij e Atl = Aii < 1 uniformly 

where Ah is the largest eigenvalue of F^Fij. 

Note that because the generalized Jacobians are zero at each step of the hierarchy, the 
hybrid contraction conditions simply define the metrics in which the discrete measurement 
incorporation steps should be contracting. As we shall see later, the flexibility offered within 
this constraint will allow us to trade-off model error vs measurement error, similarly in spirit 
to a standard Kalman filter. 



3 Extensions of the Basic Algorithm 

Discussions about full discrete measurements, use of a linear observer, nonlinear measure- 
ment, disturbance effects, and quaternion representation are offered in this section. An ob- 
server based on full measurement is described in Section |3~T1 Effects of system disturbance 
and measurement disturbance are discussed in Section l3~2l Section EOl we develop a more 
general discrete observer applicable to nonlinear measurements. Use of quaternions is studied 
in Section EH 



3.1 Computation with Full Discrete Measurement 

Assume that all states x, v, and r are actually measured, at a series of discrete instants {ti}. 
Then steps 1 and 3 are unchanged, but we can replace step 2 (the estimation of v) by the 
observer 

v = A(x) 7 

v+ = F 2i vr + (I - F 2l ) v< 
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Since we know 5x tends to zero exponentially, we have 

( &{**) = z&te^o 

[ 8vf = \ 2i 5% 

With \ 2i < 1, we have 

\ 2i e°' Au < 1 uniformly 

where \ 2i is the largest eigenvalue of F^F^. So v tends to v exponentially. 

Note that in some cases one only needs to estimate orientation x and velocity v, and that 
the discrete measurement of v may be obtained from optical flow, which can be computation- 
ally "expensive" and thus infrequent. 



3.2 Disturbance Effects 

Effects of bounded inputs and measurement disturbances can be quantified and obeserver 
gains chosen accordingly. 

Consider input disturbance d and measurement disturbance n, with ||d|| < D and ||n|| < 
N, leading to the modified system 

x = f (x) + d 

*t = K + ( k i ~ + n - x -<) 

Using the basic robustness result in J5H2I, we can quantify the corresponding quadratic 
bounds R on the estimation error 

R new =| kj | e x Au R old + | ki | ?(e X At ' - 1)+ | ki - 1 | JV 



A 



where A is the largest eigenvalue of the symmetric part of ||. 
Define the objective function (0 < kj < 1) 

F(kj) =| kj | e x At ' R old + | kj | £(e A Au — 1)+ \ kj — 1 \ N 

A 

= kj e x Au R old + k~{e^ At ' - 1) + (1 - kj)N 
A 
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Then, F(k d ) = (A + B- N)kj + N, where A = e x Au R old and B = (e x Au - l)D/\. 
We know kj should also satisfy 

kje XAu < 1 uniformly 

Define k max as an upper bound of kj. Therefore, 

^ kj ^ kfyi 

where k m = min(k max , 1). Finally, we obtain the minimum of F(kj) 

( N, when kj = if A + B - N > 

Fmin = I (A + B - N)k m + N, when kj = k m ifA + B-N<0 
{ N, when < kj < k m ifA + B-N = 

where A = Ai * i? oW and 5 = (e x Au - 1)D/X. 

When different measurements are available, the above formulas can also be used to select 
a priori the most informative measurement. This can be the case for instance for selecting 
the direction of gaze of the eyes in hopping robot [15|. This can also be the case when the 
measurements are "expensive", for instance computationally. 



Extension: The discussions above will still work when the bounds of input disturbance and 
measurement disturbance are time-varying. If ||d|| < Di and ||n|| < Ni when t E [U,U+i). 
Similar to the above, we have 

{Ni, when kj = if A + S< - N t > 

(A + Bt - Ni)k m + N u when kj = k m if A + Bt - Ni < 
when < kj < k m if A + Bi - Ni = 

where A = e~ x Au R old and B { = (e x Au - 1) A/A. 



3.3 Nonlinear measurements 



For the system x = f (x), consider the observer 

x = f fx) 



(9) 

K =Xj +g i (y l ) -gi(y;) 
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where 



We have 



y< 



<ji = § <5x 

ox 



(10) 



Defining <5z = <5x , we have 



5z 
5z- 



0. 5xr 
F5z 



Using Equation (TTOb yields 



where F 



+ f )0~ 1 and F< 



0,(1 + I^HOS," 1 . The sufficient contraction 



condition on hybrid systems can be written 

A,- e 



A At; 



< 1 



(ID 



where \ = A max (Ff Fj) and A is the largest eigenvalue of the symmetric matrix F T + F. If 
condition (ITTT) is satisfied by an appropriate choice of g i? then x will tend to x exponentially. 

A a simple illustration, consider using distance measurements instead of direct cartesian 
position measurements. In the 3-dimensional space, measure the distances from one point 
X = (xi, x 2 , x 3 ) T to four time-varying reference points A = [a 1 (t),a 2 (t),a 3 (t)] T , B = 
[6i(t), b 2 (t), b 3 (t)] T , C = [ci(t), c 2 (t), c 3 (t)] T , and D = [dx{t), d 2 {t), d 3 (t)] T , 



Vi = 


XA 


= v(a?i 


- a\ 


) 2 + (x 2 


— a 2 


) 2 + (x 3 


-a 3 ) 2 


V2 = 


XB 


= y/(xi 


-K 


2 + {x 2 


-b 2 ) 


2 + (X 3 - 


-hf 


V3 = 


XC 


= V(xi 


-ex) 


2 + (x 2 


-02) 


2 + (^3 - 


-c 3 ) 2 


2/4 = 


XD 


1= y/(xi 


— dx 


) 2 + (x 2 


- d 2 


) 2 + (*3 


-d 3 ) 2 



(12) 



The discrete-update part of observer © can be built up as below, 



x x,i 




x X,i 


-V 


r (^m) 2 


- & - (yl 


,i — y 2 ,i 


X 2,i 




X 2,i 




- (& - (yl 


,i — y?,,i 


X 3,i 




X 3,i . 


2 


(& 2 


- (^) 2 - (vl 





(13) 



where Kj is a 3 by 3 time-varying gain matrix. Using equation (fT2l yields 



(14) 
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where J, 



(du 



an 
hi 



(b 2i 
(c 2i 
(d 2i 



b 2i ) 

C 2 i, 



(hi ~ d3i) 

(c 3i - b 3i ) 

(dsi — c^i) 



where subscript i refers to the value at time tj. 



Assume Jj is non-singular. Then we can choose 

k, = ki Jr 1 (i5) 

With Equation (O, we have 

5x+ = (1 - h)5±- 

By choosing k h we can make \ satisfy the following contraction contidion that makes 5z 
tends to zero exponentially. 

\ e x At * < 1 (16) 

where Aj = (1 — h) 2 and A is the largest eigenvalue of the symmetric matrix F T + F. There- 
fore, 5x. will tend to zero, and x will tend to x exponentially. 



Remark When Jj is singular, one has 





(hi - an) (hi - 


- a 2i ) 


(h t 


- a S i) 






(cu - b u ) (c 2 i - 


-hi) 


(c 3 j 


-hi) 


= 




(du - cu) (d 2i - 


- c 2i ) 


(dzi 


- c 3i ) 




Equation (fTTt is equivalent to 
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j k 


[(bu - a u )i + (b 2i - a 2i )j + (b 3i - a 3i )k] ■ 


(cu 


-hi) 


(c 2i - 


- hi) (c 3 j - hi 






(du 


- Cu) 


(d 2i - 


- C 2 i) (d 3 i — C3i 



(17) 



(18) 



which we can write 

AB ■ (BC x CD) = 

This means that points A, B, C, and D are in the same plane, and therefore that the geometry 
does not contain enough information to infer position. 



9 



To compute velocity, one can rewrite observer © as 

v = A(x) 7 



(% l+ l) 2 



(%i+l) 
(%i+l) 2 

(S 4 , !+ l) 2 



(vli+l 



■vi,i+i) 



W3,i+1 — ^4,i+l) 



K, 



(vti) 2 



(vl. 



vh) 



} 

(19) 



where 
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and 



k, 



- dli) 

(cij - 6ii) 
(dii - cij) 



We then have 
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(d2j - C2i) 
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(C3i 
(^3i 



- 03i) 
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(Cli + l 

(dli+l 



■ hi+l) 
- Cli+l) 



(&2i+l — 02<+l) 
(C2i+1 — &2i+l) 
(^2t+l — c 2i+l) 



(C3<- 



1 — a 3i + l) 

1 — &3i+l) 
■1 — c 3i+l) 

(20)" 



Ox 







5v+ +1 = 5v l+1 - ^(tfr <+1 - Srf) = 5v i+1 - ^- J^ +1 5v dt 

which is the same as equation ©. Similarly to the second step of Section|2j this shows that v 
tends to v exponentially. 



Note that the geometry problem of going from distances to positions is solved by a dy- 
namic system, the observer, rather than explicitly at each instant. In general, one may also use 
linear measurements at some instants and nonlinear ones at others. 

Note that if a measurement is delayed, the algorithms work similarly but the actual infor- 
mation is available after the delay (i.e. the measurement is incorporated at some past time and 
the forward simulation runs instantly to the current time). 

Consider now, extending section 1X21 the effect of model and measurement errors. For the 
system x = f (x) + d, with the following nonlinear observer, 

x = f (x) + d 

x+ = K + gj(yT) - gi(y* - n) 

where/ * = y ^ . and ( d " m ° del en "° r l|d|1 < „ AT 
{ Yi — y«( x i ) I e — measurement error ||n|| <N 
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So we have 

5x=§ (5x) + 5d 

5x+ = (I + Jgi j&Uir + o7^S<Jn 
We know the quadratic bounds i? on the estimation error 

R™ w = ^ e KAu R old + £(e XA *' - 1) + v 7 ^ N 

A 

where A» = A_((I + f^)'d + gj§g)), A ei = A_((^f (^)), and A, is the 
largest eigenvalue of the symmetric part of ||. 

We can choose the most relevant discrete update function gj which will best contribute to 
improving the estimate x (i.e., to minimize R new ). 



3.4 Quaternion Representation 

Angular position can be expressed in quaternion form, avoiding representation singularities 

0. Quaternions express a rotation of angle 9 about the unit vector n as q = (cos(0/2), nsin(6 l /2)) T . 

With q = (g , q%, <?2, qz) T the quaternion vector, this leads to 




where 



and 



n = 








—UJ 2 


-U 3 







-U> 3 


LV 2 


l0 2 







-LO X 


LV 3 
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LOx 






A(q) = 



(& + ?1 - ?2 
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2(gig3 + ?o?2) 

2fe<?3 - <?o<?i) 



2(gig 2 - go<?3) 
2(gig 2 + go^s) ql~ql + ql~q 

2(gxg 3 - g g 2 ) 2(g 2 g 3 + g <?i) 

In this representation, the fact that the dynamics of q is indifferent is obvious, since CI is 
skew-symmetric. 



?2 + ? 3 2 . 
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The observers can be derived as earlier, simply by replacing © by 

q=fftq 

q+ = F u q7 + (I - F u ) qi 
based on the discrete measurements qj. Computing virtual displacements 

{8q = fl 5q 
Sqf = Fu Sq~ 
and because the dynamics of q is indifferent, we only need 

\ u e °- Ati = A M < 1 uniformly (21) 

where An is the largest eigenvalue of FjjFu. Under Condition d2"Tl) . 5q tends to zero expo- 
nentially, and q tends to q exponentially. 

The other two steps are unchanged, with A(x) being replaced by A(q). 

All the above variations and extensions can of course be combined. 



4 Simulation 



In this section, we will do a 3-dimentional simulation about system based on the discrete 
measurement Xj and the nonlinear distance measurements y l i , y 2j i, y^,i, and y 4)i , as in Section 

13 

Consider System ([!]) in the 3-dimensional case. Where 





r 2+sint "I 




" cos(2t) " 


U) = 


3+cos t 
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and 7 = 


sint 




2+sin It 




1+2 sin t 




L 3 J 




L 3 J 



Four time- varying reference points are chosen as below (all move on circular trajectories), 

A(a 1 ,a 2 ,a 3 ) f ai J~5 n 1 B(b 1 ,b 2 ,b 3 ) 



a 3 = 



\ - 60) 2 + b\ = 1 
b 3 = 
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Observer (J2J) with Aii = 1/9 is used to compute x. Observer (fl9l) with gain (l20b is used to 
compute v. Using observer (191 1 31) and gain (fT5l) . we choose ki — § to satisfy Condition (fTTT) . 
thus we can compute r. Figure [T] shows (x, v, f ) T tends to (x, v, r) T exponentially. 




2 4 6 



Figure 1 : Simulation result of computing x, v, and r with the discrete measurements Xj and r,; 



5 Concluding Remarks 

Observers similar to those developed in this paper can in principle be applied to other contin- 
uous nonlinear systems besides inertial navigation systems, although much simplification was 
afforded by exploiting the hierarchical structure of the system physics. An animation of the 
basic observer as applied to head stabilization [2| in a simulated robot hopper lfT5ll can also be 
found in http://web.mit.edu/nsl/www/hoppingj-obot.mpg 

Acknowledgement This paper benefited from stimulating discussions with Dr. Agostino 
Martinelli. 
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Appendix 

The main theorem of contraction analysis [9| can be stated as 

Theorem 1 Consider the deterministic system x = f (x, t) , where f is a smooth nonlinear 
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function. Iftheres exist a uniformly positive definite metric 

M(x,t) = @(x,t) T 0(x,t) 
5mc/? ?/za? ?/ze associated generalized Jacobian 

f = (e + ef)©- 1 

/5 uniformly negative definite, then all system trajectories then converge exponentially to a 
single trajectory, with convergence rate |A max |, where X ma x is the largest eigenvalue of the 
symmetric part ofF. The system is said to be contracting. 

It can be shown conversely that the existence of a uniformly positive definite metric with 
respect to which the system is contracting is also a necessary condition for global exponential 
convergence of trajectories. In the linear time-invariant case, a system is globally contracting 
if and only if it is strictly stable, with F simply being a normal Jordan form of the system and 
the coordinate transformation to that form. Furthermore, since 

1 <9f <9f T 

1 F s = - M- 1 (M + M — + — M) 

2 ox ax 

where is F s the symmetric part of F, all transformations © corresponding to the same M lead 
to the same eigenvalues for F s , and therefore to the same contraction rate |A maa; |. 

Consider now a hybrid case ifTH . consisting of a continuous system 

A = f(x,t) 

which is switched to a discrete system 

every Atj for one discrete step. Letting, in the same coordinate system ©, A be the largest 
eigenvalue of the symmetric matrix F T + F, and \ be the largest eigenvalue of Ff Fj (the cor- 
responding discrete-time quantity, where Fj = ©j+i-P-©." 1 , see ifTTlO . a sufficient condition 
for the overall system to be contracting is 

3a<l,Vi, 0<AV A ' l <a (22) 

Contraction theory proofs and this paper make extensive use of virtual displacements, 
which are differential displacements at fixed time borrowed from mathematical physics and 
optimization theory. Formally, if we view the position of the system at time t as a smooth 
function of the initial condition x D and of time, x = x(x D , t) , then <5x = J^- dx a . 
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